function [shatnew,signew,lh,yhat]=kf(y,H,shat,sig,G,M)
%function [shatnew,signew,lh,yhat]=kf(y,H,shat,sig,G,M)
% s is the state, and the plant equation is s(t)=Gs(t-1)+Me, where e is
% N(0,I).  The observation equation is y(t)=Hs(t).  The prior distribution for
% s is N(shat,sig).  To handle the standard Kalman Filter setup where the observation
% equation is y(t)=Hs(t)+Nu(t), expand s to become [s;v] (where v=Nu), expand H to [H I], replace
% G by [G 0;0 0], and replace M with [M 0;0 N].  The resulting model has no "error" in the
% observation equation but is equivalent to the original model with error in the state equation.
% The posterior on the new state is N(shatnew,signew) and lh is a two-dimensional vector containing
% the increments to the two component terms of the log likelihood function.  They are added 
% to form the log likelihood, but are used separately in constructing a concentrated or marginal
% likelihood. yhat is the forecast of y based on the prior.

omega=G*sig*G'+M*M';
[uo do vo]=svd(omega);
[u d v]=svd(H*uo*sqrt(do));
first0=min(find(diag(d)<1e-12));
if isempty(first0),first0=min(size(H))+1;end
u=u(:,1:first0-1);v=v(:,1:first0-1);
d=diag(d);d=diag(d(1:first0-1));
fac=vo*sqrt(do);
%% $$ y(t) - y(t)|t-1 = y(t) - H*G*s(t-1)|t-1 $$ 
yhat=y-H*G*shat;

%{
disp('=============')
y'
(H*G*shat)'
yhat'
pause
%}
ferr=(v/d)*u'*yhat;
lh=-.5*ferr'*ferr-sum(log(diag(d)));
shatnew=fac*ferr+G*shat;
signew=fac*(eye(size(v,1))-v*v')*fac';

%{
yhat = yhat(j);
%}
